;*HEADER********************************************************************
;*
;* Copyright (c) 2010-2011 Freescale Semiconductor;
;* All Rights Reserved
;*
;***************************************************************************
;*
;* THIS SOFTWARE IS PROVIDED BY FREESCALE "AS IS" AND ANY EXPRESSED OR
;* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
;* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
;* IN NO EVENT SHALL FREESCALE OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
;* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
;* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
;* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
;* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
;* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
;* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
;* THE POSSIBILITY OF SUCH DAMAGE.
;*
;**************************************************************************
;*
;* $FileName: dispatch.s$
;* $Version : 3.8.26.3$
;* $Date    : May-22-2012$
;*
;* Comments:
;*
;*   This assembler file contains functions for task scheduling
;*
;*END***********************************************************************

#include <asm_mac.h>

#include "mqx_cnfg.h"
#include "types.s"
#include "psp_prv.s"

#define __ASM__

#include "psp_cpu.h"

#include "mqx_prv.h"
#undef __ASM__

ASM_EQUATE(SVC_RUN_SCHED, 1)
ASM_EQUATE(SVC_TASK_BLOCK, 2)
ASM_EQUATE(SVC_TASK_SWITCH, 3)
ASM_EQUATE(SVC_MQX_FN, 0xaa)

 ASM_EXTERN(_mqx_kernel_data)
 ASM_EXTERN(_klog_isr_start_internal)
 ASM_EXTERN(_klog_isr_end_internal)
 ASM_EXTERN(_mqx_api_call_handler)

 ASM_CODE_SECTION(KERNEL)

 ASM_PUBLIC(_sched_start_internal)
 ASM_PUBLIC(_sched_run_internal)
 ASM_PUBLIC(_sched_check_scheduler_internal)
 ASM_PUBLIC(_sched_execute_scheduler_internal)

 ASM_PUBLIC(_task_block)
 ASM_PUBLIC(_svc_handler)
 ASM_PUBLIC(_pend_svc)
 ASM_PUBLIC(_int_kernel_isr)

 ASM_PUBLIC(_mqx_api_call)

 ASM_PUBLIC(_mem_test_and_set)
 ASM_PUBLIC(__get_PSP)
 ASM_PUBLIC(__get_MSP)
 ASM_PUBLIC(__get_PSR)

 ASM_PUBLIC(_psp_exception_return)

 SET_FUNCTION_ALIGNMENT

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _sched_start_internal
; Returned Value   : none
; Comments         : start MQX scheduler
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_sched_start_internal)
ASM_PUBLIC_FUNC(_sched_start_internal)
ASM_LABEL(_sched_start_internal)
                svc SVC_RUN_SCHED
                bx lr
ASM_PUBLIC_END(_sched_start_internal)

;-------------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_sched_run_internal)
ASM_PUBLIC_FUNC(_sched_run_internal)
ASM_LABEL(_sched_run_internal)
                svc SVC_RUN_SCHED
ASM_LABEL(_sched_run_internal_loop)
                b _sched_run_internal_loop
ASM_PUBLIC_END(_sched_run_internal)

;-------------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_sched_check_scheduler_internal)
ASM_PUBLIC_FUNC(_sched_check_scheduler_internal)
ASM_LABEL(_sched_check_scheduler_internal)
                GET_KERNEL_DATA r0

                cpsid.n i
                ldrh r1, [r0, #KD_IN_ISR]
                cbnz r1, _sched_check_scheduler_internal_end

                ldr r1, [r0, #KD_CURRENT_READY_Q]
                ldr r2, [r0, #KD_ACTIVE_PTR]
                ldr r3, [r2, #TD_MY_QUEUE]
                cmp r1, r3

                ; current task is still the active task
                beq _sched_check_scheduler_internal_end

#if MQX_ENABLE_USER_MODE
                cpsid.n i
                push {r0}
                mrs r0, IPSR
                cmp r0, #0xb
                pop {r0}
                bne _sched_check_scheduler_internal2
                push {lr}
                bl _set_pend_sv
                pop {lr}
                cpsie.n i
                bx lr
ASM_LABEL(_sched_check_scheduler_internal2)
#endif
                cpsie.n i
                svc SVC_TASK_SWITCH

ASM_LABEL(_sched_check_scheduler_internal_end)
                cpsie.n i
                bx lr
ASM_PUBLIC_END(_sched_check_scheduler_internal)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _sched_execute_scheduler_internal
; Returned Value   : none
; Comments         :
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_sched_execute_scheduler_internal)
ASM_PUBLIC_FUNC(_sched_execute_scheduler_internal)
ASM_LABEL(_sched_execute_scheduler_internal)
#if MQX_ENABLE_USER_MODE
                cpsid.n i
                push {r0}
                mrs r0, IPSR
                cmp r0, #0xb
                pop {r0}
                bne _sched_execute_scheduler_internal2
                push {lr}
                bl _set_pend_sv
                pop {lr}
                cpsie.n i
                bx lr
ASM_LABEL(_sched_execute_scheduler_internal2)
                cpsie.n i
#endif

                svc SVC_TASK_SWITCH
                bx lr
ASM_PUBLIC_END(_sched_execute_scheduler_internal)


;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _mqx_api_call
; Returned Value   : none
; Comments         :
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_mqx_api_call)
ASM_PUBLIC_FUNC(_mqx_api_call)
ASM_LABEL(_mqx_api_call)

#if MQX_ENABLE_USER_MODE
                push {lr}
                svc SVC_MQX_FN
#endif
ASM_LABEL(_mqx_api_call_end)
                b _mqx_api_call_end
ASM_PUBLIC_END(_mqx_api_call)

#if MQX_ENABLE_USER_MODE
ASM_LABEL(_mqx_api_call_handler_epilogue)
                push {r0, r1}

                ; get active task descriptor
                GET_KERNEL_DATA r0
                ldr r0, [r0, #KD_ACTIVE_PTR]

                ; set task flag to privilege mode
                ldr r1, [r0, #TD_FLAGS]

                tst r1, #0x10   ; #MQX_USER_TASK
                beq _mqx_api_call_handler_epilogue_end

                orr r1, r1, #TASK_USER_MODE
                str r1, [r0, #TD_FLAGS]

                ; user mode, proc stack
                mov r0, #3
                msr CONTROL, r0

ASM_LABEL(_mqx_api_call_handler_epilogue_end)
                pop {r0, r1, pc}

ASM_LABEL(_mqx_api_call_handler_prologue)
                mrs r1, PSP

                ; modify stack - return adress from svc
                ldr r0, =_mqx_api_call_handler
                ;bic r0, r0, #1
                str r0, [r1, #24]                                        ; set stacked PC to requested mqx fn

                ; modify stack - return address from mqx api
                ldr r0, =_mqx_api_call_handler_epilogue
                ;bic r0, r0, #1
                str r0, [r1, #20]                                        ; set stacked LR to _mqx_fn_handler_epilogue

                ; get active task descriptor
                GET_KERNEL_DATA r0
                ldr r0, [r0, #KD_ACTIVE_PTR]

                ; set task flag to privilege mode
                ldr r1, [r0, #TD_FLAGS]
                bic r1, r1, #TASK_USER_MODE
                str r1, [r0, #TD_FLAGS]

                ; privilege mode
                mov r0, #0
                msr CONTROL, r0

                cpsie.n i

                bx lr

#endif

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _task_block
; Returned Value   : none
; Comments         : task block function - block actual task - switch to another
;
;END*---------------------------------------------------------------------------
ASM_PUBLIC_BEGIN(_task_block)
ASM_PUBLIC_FUNC(_task_block)
ASM_LABEL(_task_block)

                svc SVC_TASK_BLOCK
                bx lr
ASM_PUBLIC_END(_task_block)


;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _svc_handler
; Returned Value   : none
; Comments         : SVC handler - called after SVC instruction
;                    call MQX scheduler functions
;
;END*---------------------------------------------------------------------------

ASM_LABEL(_svc_handler)
                cpsid.n i

                tst lr, #0x4                        ; test EXC_RETURN number in LR bit 2
                ite eq                              ; if zero (equal) then
                mrseq r1, MSP                       ; Main Stack was used, put MSP in R1
                mrsne r1, PSP                       ; else, Process Stack was used, put PSP in R1
;                ldr r0, [r1, #0]                   ; get stacked R0 from stack
                ldr r1, [r1, #24]                   ; get stacked PC from stack
                ldrb r1, [r1, #-2]                  ; get the immediate data from the instruction

#if MQX_ENABLE_USER_MODE
                cmp r1, #SVC_MQX_FN
                bne svc_task_switch

                ;tst lr, #0x4                       ; test EXC_RETURN number in LR bit 2
                ;ite eq                             ; if zero (equal) then
                ;mrseq r0, MSP                      ; Main Stack was used, put MSP in R1
                ;mrsne r0, PSP

                b _mqx_api_call_handler_prologue
#endif

ASM_LABEL(svc_task_switch)
                cmp r1, #SVC_TASK_SWITCH
                bne svc_run_sched

                push {lr}
                bl _set_pend_sv
                pop {lr}

                b svc_handler_end

ASM_LABEL(svc_run_sched)
                cmp r1, #SVC_RUN_SCHED
                bne svc_task_block

                GET_KERNEL_DATA r0
                b first_run

ASM_LABEL(svc_task_block)
                cmp r1, #SVC_TASK_BLOCK
                bne svc_handler_end

                GET_KERNEL_DATA r0
                ldr r1, [r0, #KD_ACTIVE_PTR]        ; get active task descriptor

                ; set task block bit
                ldr r2, [r1, #TD_STATE]
                orr r2, r2, #1                      ; TODO change from number to define
                str r2, [r1, #TD_STATE]

                push {lr}

#if MQX_KERNEL_LOGGING
                KLOG r0, ASM_PREFIX(_klog_block_internal)        ; kernel log this function
#endif // MQX_KERNEL_LOGGING
                ; remove active task from ready que
                ldr r2, [r1, #TD_TD_PREV]           ; get ptr to ready_q structure
                ldr r3, [r1, #TD_TD_NEXT]
                str r3, [r2, #RQ_HEAD_READY_Q]
                str r2, [r3, #TD_TD_PREV]

                bl _set_pend_sv
                pop {lr}

ASM_LABEL(svc_handler_end)
                cpsie.n i
                bx lr

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _set_pend_sv
; Returned Value   : none
; Comments         : This functions set pending sv flag - request for PendSV
;                    request for task switch
;
;END*---------------------------------------------------------------------------

ASM_LABEL(_set_pend_sv)
                push {r0-r2, lr}

                ; get PendSV flag
                ldr r0, =0xE000ED00
                ldr r1, [r0, #4]                    ; 0xE000ED04
                tst r1, #0x10000000
                bne _set_pend_sv_end

                mrs r2, BASEPRI
                sub r2, r2, #0x10

                ; set priority for PendSV
                strb r2, [r0, #0x22]                ; 0xE000ED22

                ; set PendSV flag
                ldr r1, =0x10000000                 ; PENDSVSET
                str r1, [r0, #4]                    ; 0xE000ED04

ASM_LABEL(_set_pend_sv_end)
                pop {r0-r2, pc}

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _pend_svc
; Returned Value   : none
; Comments         : PendSV handler - task switch functionality
;
;END*---------------------------------------------------------------------------

; Pending Service Call
ASM_LABEL(_pend_svc)
                cpsid.n i

                ; store active task registers
                mrs r12, PSP                        ; get process stack pointer for active task

                ldr r1, =0xE000ED20                 ; SHPR3
                ldrb r2, [r1, #2]

                mrs r3, BASEPRI
                stmdb r12!, {r2-r11, lr}            ; store remaining registers

                GET_KERNEL_DATA r0
                ldr r3, [r0, #KD_ACTIVE_PTR]        ; get active task descriptor
                str r12, [r3, #TD_STACK_PTR]        ; store task SP to task descriptor td

#if MQXCFG_ENABLE_FP && PSP_HAS_FPU
                ldr r1, [r3, #TD_FLAGS]

                ; check for fpu flag in TD
                tst r1, #FP_TASK_MASK
                beq fpu_store_end

                ; store FPU registers
                ldr r12, [r3, #TD_FLOAT_CONTEXT_PTR]    ; get task fpu context address
                ldr r2, =0xE000EF34                     ; FPCCR
                ldr r1, [r2], #4                        ; FPCCR
                str r1, [r12], #4

                ldr r1, [r2], #4                        ; FPCAR
                str r1, [r12], #4

                ldr r1, [r2], #4                        ; FPDSCR
                str r1, [r12], #4

                vmrs r1, FPSCR
                str r1, [r12], #8                       ; FPSCR

                vstm r12, {s0-s31}                      ; restore fpu registers
ASM_LABEL(fpu_store_end)
#endif // MQXCFG_ENABLE_FP && PSP_HAS_FPU

ASM_LABEL(first_run)
                cpsid.n i
                ldr r1, [r0, #KD_CURRENT_READY_Q]   ; get current ready q
ASM_LABEL(find_noempty_que)
                ldr r2, [r1, #0]                    ; address of first td
                cmp r2, r1                          ; ready_q structure itself?
                bne switch_task
                ldr r1, [r1, #RQ_NEXT_Q]            ; try next queue
                movs r1, r1
                bne find_noempty_que

                ; r1 is 0 -> empty
ASM_LABEL(no_one_to_run)
                ; TODO set system task ???

                ; enable all interrupts (r1 = 0)
                ; TODO maybe (maybe not necessary) restore PendSV priority and BASEPRI after wfi
                msr BASEPRI, r1

                ldr r1, =0xE000ED20                    ; SHPR3
                ldr r2, =0xff
                strb r2, [r1, #2]

                ; wait for interrupt
                cpsie.n i
                wfi
                cpsid.n i

                ; TODO check r0, must be kernel data
                ldr r1, [r0, #KD_READY_Q_LIST]      ; get first que from ready list
                b find_noempty_que

ASM_LABEL(switch_task)
                ; update kernel structures
                str r1, [r0, #KD_CURRENT_READY_Q]   ; store addr for active que
                str r2, [r0, #KD_ACTIVE_PTR]        ; active task descriptor

                ldrh r3, [r2, #TD_TASK_SR]
                strh r3, [r0, #KD_ACTIVE_SR]        ; restore priority mask for enabled interrupt for active task

#if MQX_ENABLE_USER_MODE || (MQXCFG_ENABLE_FP && PSP_HAS_FPU)
                ldr r1, [r2, #TD_FLAGS]
#endif

#if MQXCFG_ENABLE_FP && PSP_HAS_FPU
                ; check for fpu flag in TD
                tst r1, #FP_TASK_MASK
                beq fpu_restore_end

                ; check if last fpu task is different
                ldr r3, [r0, #KD_FP_ACTIVE_PTR]
                cmp r2, r3
                beq fpu_restore_end

                str r2, [r0, #KD_FP_ACTIVE_PTR]

                ; restore FPU registers
                ldr r12, [r2, #TD_FLOAT_CONTEXT_PTR]    ; get task fpu context address
                ldr r4, =0xE000EF34                     ; FPCCR
                ldr r3, [r12], #4
                str r3, [r4], #4                        ; FPCCR

                ldr r3, [r12], #4
                str r3, [r4], #4                        ; FPCAR

                ldr r3, [r12], #4
                str r3, [r4], #4                        ; FPDSCR

                ldr r3, [r12], #8                       ; FPSCR
                vmsr FPSCR, r3

                vldm r12!, {s0-s31}                     ; restore fpu registers
ASM_LABEL(fpu_restore_end)
#endif // MQXCFG_ENABLE_FP && PSP_HAS_FPU

                ; activate task, restore registers
                ldr r12, [r2, #TD_STACK_PTR]        ; get task SP
                ldmia r12!, {r2-r11, lr}            ; restore registers
                msr BASEPRI, r3

                msr PSP, r12                        ; restore process stack pointer for task

#if MQX_KERNEL_LOGGING
                KLOG r0, ASM_PREFIX(_klog_context_switch_internal)  ; do kernel logging
#endif

#if MQX_ENABLE_USER_MODE
                tst r1, #TASK_USER_MODE             ; r1 still contain TD_FLAGS, check for user mode task
                ite eq
                moveq r0, #0                        ; privilege mode
                movne r0, #1                        ; user mode
                msr CONTROL, r0
#endif
                ; clear PendSV flag - fix for late arriving optimalization issue
                ldr r0, =0xE000ED00
                ldr r1, =0x08000000                 ; PENDSVCLR
                str r1, [r0, #4]                    ; 0xE000ED04

                cpsie.n i
                bx lr

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : _int_kernel_isr
; Returned Value   : none
; Comments         : MQX kernel interrupt handler
;
;END*---------------------------------------------------------------------------

; kernel interrupt handler
ASM_LABEL(_int_kernel_isr)
                cpsid.n i
                push {lr}

                GET_KERNEL_DATA r3              ; get the kernel data address

                ; increment in interrupt counter
                ldrh r1, [r3, #KD_IN_ISR]
                add r1, r1, #1
                strh r1, [r3, #KD_IN_ISR]

                ; create interrupt content
                ldr r0, =0                      ; error code (set 0)
                push {r0}                       ; store in stack
                mrs r2, BASEPRI                 ; actual priority
                mrs r1, IPSR                    ; exception number
                ldr r0, [r3, #KD_INTERRUPT_CONTEXT_PTR] ; previous interrupt content
                push {r0-r2}                    ; store in stack

                mrs r0, MSP                     ; get address of interrupt content
                str r0, [r3, #KD_INTERRUPT_CONTEXT_PTR] ; store address of actual interrupt content in kernel data

                mov r0, r1

#if MQX_KERNEL_LOGGING
                ldr r1, [r3, #KD_LOG_CONTROL]
                tst r1, #0x00000001
                beq _isr_no_logging

                ; prepare parameters for klog function
                push {r0-r3}
                bl ASM_PREFIX(_klog_isr_start_internal)
                pop {r0-r3}
ASM_LABEL(_isr_no_logging)
#endif // MQX_KERNEL_LOGGING
                ; get C function address from MQX interrupt table
                ; r0 - interrupt number

                ; check if isr is in table range
                ldr r2, [r3, #KD_LAST_USER_ISR_VECTOR]

                ; cbz r2, _isr_run_default       ; isr not used (cbz not working in CW)
                cbnz r2, _isr_skip_run_default1  ; isr not used (this is CW workaround)
                b _isr_run_default
ASM_LABEL(_isr_skip_run_default1)

                cmp r0, r2
                bhi _isr_run_default

                ldr r2, [r3, #KD_FIRST_USER_ISR_VECTOR]
                subs r1, r0, r2                         ; r1 = actual exception number in table
                blt _isr_run_default

#if MQX_SPARSE_ISR_TABLE

ASM_LABEL(_int_kernel_isr_vect_ok)
                ldr r2, [r3, #KD_INTERRUPT_TABLE_PTR]   ; get the interrupt table pointer
                lsr r1, r1, #MQX_SPARSE_ISR_SHIFT
                lsl r1, r1, #2

                ldr r1, [r2, r1]                        ; get address of first isr in linked list

                ; cbz r1, _isr_run_default              ; isr not used (cbz not working in CW)
                cbnz r1, _isr_skip_run_default2         ; isr not used (this is CW workaround)
                b _isr_run_default
ASM_LABEL(_isr_skip_run_default2)

                ; r1 - address of first isr in linked list
ASM_LABEL(_isr_search)
                ldr r2, [r1, #HASH_ISR_NUM]             ; get isr num
                cbz r2, _isr_search_fail

                cmp r2, r0                              ; compare isr number in record with actual isr number
                beq _isr_search_suc

                ldr r1, [r1, #HASH_ISR_NEXT]            ; next vector in list
                tst r1, r1
                bne _isr_search

ASM_LABEL(_isr_search_fail)
                b _isr_run_default

ASM_LABEL(_isr_search_suc)
                ldr r0, [r1, #HASH_ISR_DATA]            ; move notifier data into r0 = first parameter in C func
                ldr r2, [r1, #HASH_ISR_ADDR]            ; move interrupt function address to r2

#else /* MQX_SPARSE_ISR_TABLE */

ASM_LABEL(_int_kernel_isr_vect_ok)
                ; calculate offset in table
                ; each table entry is 12 bytes in size
                mov r2, #12
                mul r1, r2, r1

                ldr r2, [r3, #KD_INTERRUPT_TABLE_PTR]         ; pointer to interrupt table begin
                add r1, r1, r2                                ; get address of entry in table
                ldr r2, [r1, #0]
                ldr r0, [r1, #IT_APP_ISR_DATA]          ; move notifier data into r0 = first parameter in C func
#endif /* MQX_SPARSE_ISR_TABLE */

ASM_LABEL(_isr_execute)
                ; r0 = first parameter in C func
                ; r2 contain interrupt function address

                cpsie.n i
                push {r3}

                blx r2

ASM_LABEL(_int_kernel_isr_epilog)

                pop {r3}
                cpsid.n i

ASM_LABEL(_int_kernel_isr_return_internal)

#if MQX_KERNEL_LOGGING
                ldr r1, [r3, #KD_LOG_CONTROL]
                tst r1, #0x00000001
                beq _isr_return_no_logging

                mrs r0, IPSR                    ; exception number

                ; prepare parameters for klog function
                push {r0-r3}
                bl ASM_PREFIX(_klog_isr_end_internal)
                pop {r0-r3}
ASM_LABEL(_isr_return_no_logging)
#endif /* MQX_KERNEL_LOGGING */

                ; remove interrupt content
                pop {r0-r2}
                str r0, [r3, #KD_INTERRUPT_CONTEXT_PTR] ; update pointer to interrupt content

                pop {r0}                                ; error code

                ; decrement interrupt counter
                ldrh r1, [r3, #KD_IN_ISR]
                subs r1, r1, #1
                strh r1, [r3, #KD_IN_ISR]

                ; check for reschedule
                ; check preemtion
                ldr r2, [r3, #KD_ACTIVE_PTR]    ; TD pointer
                ldr r0, [r2, #TD_FLAGS]
                tst r0, #TASK_PREEMPTION_DISABLED
                bne _isr_return_end

                cbnz r1, _isr_return_end        ; waiting another isr, do not reschedulle

                ; if a different TD at head of current readyq, then we need to run the scheduler
                ; check for reschedule
                ldr r1, [r3, #KD_CURRENT_READY_Q]
                ldr r1, [r1]
                cmp r1, r2
#ifdef __CODEWARRIOR__
                it ne
                bl _set_pend_sv
#else
                it ne
                blne _set_pend_sv
#endif

ASM_LABEL(_isr_return_end)
                cpsie.n i
                pop {pc}

ASM_LABEL(_isr_run_default)
                ; r0 - interrupt number

                ldr r2, [r3, #KD_DEFAULT_ISR]

                b _isr_execute

;FUNCTION*-------------------------------------------------------------------
;
; Function Name    : _psp_exception_return
; Returned Value   : none
; Comments         : This functions returns us from an isr exception
;
;END*----------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_psp_exception_return)
ASM_PUBLIC_FUNC(_psp_exception_return)
ASM_LABEL(_psp_exception_return)
                cpsid.n i
                push {r0, r4-r7}    ; store parameter and registers which are used for copy and cpu do not store if

                GET_KERNEL_DATA r3

                ; copy actual int context + 4WORDS -> prev int context - (1 + 8)WORDS
                ldr r12, [r3, #KD_INTERRUPT_CONTEXT_PTR]    ; get actual int context
                add r12, r12, #16               ; calculate source address

                ldr lr, [r12], #4               ; lr - return from exception... (0xfffffff1)

                ; modify return address (PC) in exception stack frame (kernel isr epilog)
                ldr r0, =_int_kernel_isr_epilog
                bic r0, r0, #1
                str r0, [r12, #24]

                ldmia r12, {r0-r7}              ; read exception stack frame

                pop {r12}                       ; get prev int context address (function parameter - r0)

                ; destination stack address (4B + 32B - exception stack frame)
                sub r12, r12, #4    ;#36
                stmdb r12!, {r0-r7}

                ; restore modified registers which are not restored by cpu
                pop {r4-r7}

                       msr MSP, r12   ; update MSP (main stack pointer)

                cpsie.n i

                bx lr
ASM_PUBLIC_END(_psp_exception_return)

/*******************************************************************************
 * test support
 ******************************************************************************/

;FUNCTION*-------------------------------------------------------------------
;
; Function Name    : _mem_test_and_set
; Returned Value   : previous value of location
; Comments         :
;   This function tests a byte location, and if AND 0x80 is 0 (7bit is set !!!), sets it to 0x80.
;   It returns the previous value of the byte (0 or 0x80).
;
;END*----------------------------------------------------------------------

ASM_PUBLIC_BEGIN(_mem_test_and_set)
ASM_PUBLIC_FUNC(_mem_test_and_set)
ASM_LABEL(_mem_test_and_set)
                push {r1, lr}

                ldrb r1, [r0]
                tst r1, #0x80
                ittte eq
                orreq r1, r1, #0x80
                strbeq r1, [r0]
                eoreq r0, r0
                movne r0, #0x80

                pop {r1, pc}
ASM_PUBLIC_END(_mem_test_and_set)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_PSP
; Returned Value   : none
; Comments         : This functions returns PSP register value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__get_PSP)
ASM_PUBLIC_FUNC(__get_PSP)
ASM_LABEL(__get_PSP)
                mrs r0, PSP
                bx lr
ASM_PUBLIC_END(__get_PSP)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_MSP
; Returned Value   : none
; Comments         : This functions returns MSP register value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__get_MSP)
ASM_PUBLIC_FUNC(__get_MSP)
ASM_LABEL(__get_MSP)
                mrs r0, MSP
                bx lr
ASM_PUBLIC_END(__get_MSP)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __set_MSP
; Returned Value   : none
; Comments         : This functions set MSP register value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__set_MSP)
ASM_PUBLIC_FUNC(__set_MSP)
ASM_LABEL(__set_MSP)
                msr MSP, r0
                bx lr
ASM_PUBLIC_END(__set_MSP)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_PSR
; Returned Value   : none
; Comments         : This functions returns PSR (procesor status register) value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__get_PSR)
ASM_PUBLIC_FUNC(__get_PSR)
ASM_LABEL(__get_PSR)
        #ifdef __CWARM__
                 mrs r0, XPSR
        #else
                 mrs r0, PSR
        #endif
                 bx lr
ASM_PUBLIC_END(__get_PSR)

;IAR defines following functions internally
#ifndef __IAR_SYSTEMS_ASM__

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __enable_interrupt
; Returned Value   : none
; Comments         : This functions enables interrupts
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__enable_interrupt)
ASM_PUBLIC_FUNC(__enable_interrupt)
ASM_LABEL(__enable_interrupt)
                 cpsie.n i
                 bx lr
ASM_PUBLIC_END(__enable_interrupt)


;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __disable_interrupt
; Returned Value   : none
; Comments         : This functions disables interrupts
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__disable_interrupt)
ASM_PUBLIC_FUNC(__disable_interrupt)
ASM_LABEL(__disable_interrupt)
                 cpsid.n i
                 bx lr
ASM_PUBLIC_END(__disable_interrupt)


 ASM_PUBLIC(__get_LR)
 ASM_PUBLIC(__get_PC)
 ASM_PUBLIC(__enable_interrupt)
 ASM_PUBLIC(__disable_interrupt)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_LR
; Returned Value   : none
; Comments         : This functions returns LR register value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__get_LR)
ASM_PUBLIC_FUNC(__get_LR)
ASM_LABEL(__get_LR)
                mov r0, lr
                bx lr
ASM_PUBLIC_END(__get_LR)


;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_PC
; Returned Value   : none
; Comments         : This functions returns LR register value
;
;END*---------------------------------------------------------------------------

ASM_PUBLIC_BEGIN(__get_PC)
ASM_PUBLIC_FUNC(__get_PC)
ASM_LABEL(__get_PC)
                mov r0, PC
                bx lr
ASM_PUBLIC_END(__get_PC)

;FUNCTION*----------------------------------------------------------------------
;
; Function Name    : __get_PC
; Returned Value   : none
; Comments         : This functions returns LR register value
;
;END*---------------------------------------------------------------------------

 ASM_PUBLIC(__get_CONTROL)

ASM_PUBLIC_BEGIN(__get_CONTROL)
ASM_PUBLIC_FUNC(__get_CONTROL)
ASM_LABEL(__get_CONTROL)
                mrs r0, CONTROL
                bx lr
ASM_PUBLIC_END(__get_CONTROL)

#endif  /*__IAR_SYSTEMS_ASM__*/

 ASM_ALIGN(4)
 ASM_END


